/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/
#include "scene/include/msg_verify.h"

#include <cmath>
#include <set>
#include <unordered_map>
#include <vector>

#include <glog/logging.h>

#include "v2xpb-asn-bsm.pb.h"
#include "v2xpb-asn-map.pb.h"
#include "v2xpb-asn-position.pb.h"
#include "v2xpb-asn-spat.pb.h"

namespace v2x {
namespace scene {

bool MsgVerify::VerifyBsm(const ::v2xpb::asn::Bsm& bsm) {
  if (!bsm.has_speed()) {
    LOG(ERROR) << "invalid bsm has no speed, return false";
    return false;
  }
  if (!bsm.has_pos()) {
    LOG(ERROR) << "invaild bsm has no position of host vehicle, return false";
    return false;
  } else {
    auto pos = bsm.pos();
    if (!pos.has_xyz()) {
      LOG(ERROR) << "invalid bsm has no xyz of position, return false";
      return false;
    }
  }
  if (!bsm.has_heading()) {
    LOG(ERROR) << "invalid bsm has no heading, return false";
    return false;
  }
  if (bsm.has_accel_set()) {
    auto accelerate_set = bsm.accel_set();
    if (!accelerate_set.has_lon()) {
      LOG(WARNING) << "bsm has no accelerate";
    }
  } else {
    LOG(WARNING) << "bsm has no accelerate";
  }
  LOG(INFO) << "bsm verified success";
  return true;
}
bool MsgVerify::VerifyMap(const ::v2xpb::asn::Map& map) {
  if (map.nodes_size() == 0) {
    LOG(ERROR) << "invalid map has no map_node, return false";
    return false;
  }
  for (int i = 0; i < map.nodes_size(); ++i) {
    const auto& intersection = map.nodes(i);
    if (!intersection.has_id()) {
      LOG(ERROR) << "invalid map node has no id, return false";
      return false;
    }
    if (!intersection.id().has_region()) {
      LOG(ERROR) << "invalid map has no region id, return false";
      return false;
    }
    if (!intersection.id().has_id()) {
      LOG(ERROR) << "invalid map has no node id, return false";
      return false;
    }
    if (intersection.links_size() == 0) {
      LOG(ERROR) << "invaild map has no map_link, return false";
      return false;
    }
    for (int j = 0; j < intersection.links_size(); ++j) {
      const auto& link = intersection.links(j);
      if (link.lanes_size() == 0) {
        LOG(ERROR) << "invalid map has no map_lane, return false";
        return false;
      }
      for (int k = 0; k < link.lanes_size(); ++k) {
        const auto& lane = link.lanes(k);
        if (lane.has_width() && lane.width() < 0) {
          LOG(ERROR) << "lane's width is negtive, return false";
          return false;
        }
        LOG(INFO) << "lane position size is " << lane.positions_size();
        if (lane.positions_size() < 2 || lane.positions_size() > 31) {
          LOG(ERROR) << "invalid map position size not in [2,31], return false";
          return false;
        }
        for (int t = 0; t < lane.positions_size(); ++t) {
          const auto& pose = lane.positions(t);
          if (!pose.has_xyz()) {
            LOG(ERROR) << "invalid map has no xyz pose, return false";
            return false;
          }
          const auto& xyz = pose.xyz();
          if (!xyz.has_x() || !xyz.has_y()) {
            LOG(ERROR) << "invalid map has no x or y, return false";
            return false;
          }
        }
        if (lane.connections_size() == 0) {
          LOG(ERROR) << "the size of lane connections is 0, return false";
          return false;
        }
        for (int t = 0; t < lane.connections_size(); ++t) {
          const auto& connect = lane.connections(t);
          if (!connect.has_phase_id()) {
            LOG(ERROR) << "invalid map has no phase id, return false";
            return false;
          }
        }
      }
    }
  }
  LOG(INFO) << "map verified success";
  return true;
}
bool MsgVerify::VerifySpat(const ::v2xpb::asn::Spat& spat) {
  if (spat.intersections_size() == 0) {
    LOG(ERROR) << "invalid spat has no intersection, return false";
    return false;
  }
  for (int i = 0; i < spat.intersections_size(); ++i) {
    auto& intersection = spat.intersections(i);
    if (!intersection.has_node_region()) {
      LOG(ERROR) << "invalid spat has no region id, return false";
      return false;
    }
    if (!intersection.has_node_id()) {
      LOG(ERROR) << "invalid spat has no node id, return false";
      return false;
    }
    if (intersection.phases_size() == 0) {
      LOG(ERROR) << "invalid spat hs no phases, return false";
      return false;
    }
    for (int j = 0; j < intersection.phases_size(); ++j) {
      auto& phase = intersection.phases(j);
      if (!phase.has_id() || phase.phase_state_size() == 0) {
        LOG(ERROR) << "invalid spat has no id or phase_state, return false";
        return false;
      }
      for (int k = 0; k < phase.phase_state_size(); ++k) {
        auto& spat_phase = phase.phase_state(k);
        if (!spat_phase.has_color() || !spat_phase.has_timing_start() ||
            !spat_phase.has_timing_end() || !spat_phase.has_timing_duration()) {
          LOG(ERROR) << "invalid phase has no color or time, return false";
          return false;
        }
        if (k != 0 &&
            spat_phase.timing_start() + spat_phase.timing_duration() !=
                spat_phase.timing_end()) {
          LOG(ERROR) << "wrong phase time, return false";
          return false;
        }
        if (k != 0 &&
            spat_phase.timing_start() < phase.phase_state(k - 1).timing_end()) {
          LOG(ERROR) << "previous phase's timing_end is less than current "
                        "phase's timing_start, return false";
          return false;
        }
      }
      if (phase.phase_state(0).timing_start() != 0) {
        LOG(ERROR) << "the first spat's timing start is not 0, return false";
      }
    }
  }
  return true;
}

std::shared_ptr<::v2xpb::asn::SpatPhase> MsgVerify::GetSpatPhaseId(
    const ::v2xpb::asn::SpatIntersection& spat_intersection,
    const ::v2xpb::asn::MapLane& lane) {
  auto spat_phase = std::make_shared<::v2xpb::asn::SpatPhase>();
  std::set<int> phase_id;
  std::vector<::v2xpb::asn::SpatPhase> phase_vec;
  for (int i = 0; i < lane.connections_size(); ++i) {
    phase_id.insert(lane.connections(i).phase_id());
  }
  for (int i = 0; i < spat_intersection.phases_size(); ++i) {
    auto phase = spat_intersection.phases(i);
    if (phase_id.count(phase.id()) != 0) {
      phase_vec.emplace_back(phase);
    }
  }
  if (phase_vec.size() == 0) {
    LOG(ERROR) << "the size of spat on the lane is 0, return";
    return nullptr;
  }
  if (phase_vec.size() == 1) {
    spat_phase->CopyFrom(phase_vec[0]);
    LOG(INFO) << "the phase id of lane is spat_phase is " << spat_phase->id();
    return spat_phase;
  }

  for (int i = 1; i < phase_vec.size(); ++i) {
    if (phase_vec[i].phase_state_size() != phase_vec[0].phase_state_size()) {
      LOG(ERROR) << "sizes of phase_states are different, return";
      return nullptr;
    }
    for (int j = 0; j < phase_vec[0].phase_state_size(); ++j) {
      auto phase_state_0 = phase_vec[0].phase_state(j);
      auto phase_state_i = phase_vec[i].phase_state(j);
      if (phase_state_0.color() != phase_state_i.color()) {
        LOG(ERROR) << "spat color of phases are different, return";
        return nullptr;
      }
      if (phase_state_0.timing_start() != phase_state_i.timing_start()) {
        LOG(ERROR) << "spat timing start of phases are different, return";
        return nullptr;
      }
      if (phase_state_0.timing_end() != phase_state_i.timing_end()) {
        LOG(ERROR) << "spat timing end of phases are different, return";
        return nullptr;
      }
      if (phase_state_0.timing_duration() != phase_state_i.timing_duration()) {
        LOG(ERROR) << "spat timing duration of phases are different, return";
        return nullptr;
      }
    }
  }

  spat_phase = std::make_shared<::v2xpb::asn::SpatPhase>(phase_vec[0]);
  return spat_phase;
}

}  // namespace scene
}  // namespace v2x
